#include <Wire.h>
#include <Servo.h>
#include <Depecabot3.h>

Depecabot robot;

unsigned int estado=0;
unsigned int dist=0;
unsigned long timer;

void setup(void)
{
  robot.init ();

  robot.servo1.attach(SERVO);            //Inicializacion servo
  robot.servo1.write(90);
}
void loop(void)
{
  dist=robot.gp2ymm(GP2);  
  switch(estado)
  {
  case 0:                  //Estado 0: Avanza hasta encontrar algo a menos de 10cm
    {
      robot.motores(100,100);
      if(dist<250)
      {
        robot.motores(0,0);
        timer=millis();    //Reinicia timer.
        estado=1;
      }
      break;
    }

  case 1:                  //Estado 1: Retrocede durante 1 segundo
    {
      robot.motores(-75,-75);
      if(millis()-timer>1600)
      {
        estado=5;
        timer=millis();    //Reinicia timer.
        estado=2;
      }

      break;
    }
  case 2:                  //Estado 2: Gira sobre si mismo durante 0,4 segundos
    {
      robot.motores(75,-75);
      if(millis()-timer>400)
      {
        estado=0;
      }
      break;
    }
  }
}
